/*
Work in this file belongs to STD. LINE FOLLOWING group. 
Other groups are not allowed to make changes on this file. 

During development, you can use your own test code that
includes your own setup() and loop() functions but they 
should not be included when you are submitting your changes. 
*/

float slSetPoint=3;
float slMeasurement;
float slError;
float slOutput;
//float slSolMotor=0;
//float slSagMotor=0;    
float slKp=305;

int slIndex=0;
boolean slTurnStatus=0;
boolean slLastTurnStatus=0;

void stdLineFollowing(){
  slSensorState1 = digitalRead(12);
  slSensorState2 = digitalRead(11);
  slSensorState3 = digitalRead(10);
  slSensorState4 = digitalRead(9);
  slSensorState5 = digitalRead(8);
  
  slLastTurnStatus=slTurnStatus;

   
  
  if(slSensorState1==1 || slSensorState5==1) {    //yol ayrımı
    slTurnStatus=true;
    if(directionArray[slIndex]=='L'){
      slSensorState4=0;
    }
    else if(directionArray[slIndex]=='R'){
      slSensorState2=0; 
    }
    else if(directionArray[slIndex]=='S'){
      //do nothing
    }  
  }
  else{    //normal gidiş
    slTurnStatus=false;
    }
    slMeasurement=(float)((2*(slSensorState2))+(3*(slSensorState3))+(4*(slSensorState4)))/((1*(slSensorState2))+(1*(slSensorState3))+(1*(slSensorState4)));
    slError=slSetPoint-slMeasurement;
    slOutput= slKp*slError;
    
    slSolMotor=(102.5-(slOutput/2));
    slSagMotor=(102.5+(slOutput/2));
  
  if(slLastTurnStatus==1 && slTurnStatus==0){
    slIndex=slIndex+1;
  }
}
